I'm building a quadruped (4 legs 3 DOF) for my major work in electronics at high school. I need to know the displacement of the robot and I can't use an encoding wheel. I've done my research and I found a system where I use an accelerometer, gyroscope and a magnetometer to determine the displacement but I'm not sure how to code it.

I'm using Arduino and will use compatible sensors.

I would like to be able to implement something similar to this video which demonstrates the principles evaluated in this paper. I don't need to map the data on a screen, just be able to read its displacement from its last recorded position.

EG. (Read Position) ---> (Do "stuff") ---> (Read Position) ---> (Calculate displacement caused by "stuff")